/******************
 * recordLayer
 * 
 * For this layer, I envision a bipedal skeleton with various pieces of it acting as buttons
 *    -The Head (Direct Input)
 *    -Individual Arms (Interpreted Input & Direct Input for comparison) 
 *    -Body Twist (Interpreted Input)
 *    -Crouch
 *    -Individual Legs
 ******************/

class recordLayer extends Layer
{
  int fileNum = 0;
  ArrayList conList;
  ArrayList launchButtons;

  recordLayer()
  {
    super(44,44,width-44,height-44,0);
    RESIZEENABLED = false;
    MINX = 44;
    MINY = 44;
    
    launchButtons = new ArrayList();
    scanControllers(); 
  }
  
  //scan the controllers folder for controller profiles  
  void scanControllers()
  {
    conList = new ArrayList();    

    String path = System.getProperty("user.dir") + "\\controllers";
    File dir = new File(path);   
    String[] children = dir.list();
    for(int i = 0; i < children.length; i++)
    {
      conList.add(new Controller(children[i],0));
    }
  }
  
  boolean mDraw()
  {
    pg.beginDraw();
    pg.background(BORDERCOLOR);
    pg.fill(FILLCOLOR);
    pg.noStroke();
    pg.rect(3,3,pg.width-6,pg.height-6);
    
    pg.textFont(font,28);
    pg.textAlign(CENTER);
    pg.fill(TEXTCOLOR);
    pg.text("Welcome to the RECORD tab!",w/2,30);
    pg.textAlign(LEFT);
    pg.textFont(font,20);
    pg.text("Available Controllers:",10,75);
    pg.textFont(font,14);

    launchButtons = new ArrayList();
    //Controllers don't update mid-applet...they have to be plugged in at startup.
    //launchButtons.add(new LaunchButton("Rescan",new Message("0",ACTIVELAYER,24),x,y,200,52,45,25)); 
    
    int index = 0;
    for(int i = 0; i < conList.size(); i++)
    {
      Controller c = (Controller)conList.get(i);
      boolean connected = true;
      String addOn = "";      
      if(c.button.length == 0 && c.slider.length==0){
        connected = false;
        addOn = " (NOT CONNECTED)";
      }
      pg.text(c.controllerName + addOn,10,100 + 25*index++);
      /*
      if(connected && c.controllerName.equals("Logitech Dual Action"))
      {
        launchButtons.add(new LaunchButton("Launch",new Message("2",ACTIVELAYER,24),x,y,10,85 + 25*index,45,20));
        pg.text("Basic Controller Display",60,100 + 25*index++);
      }
      if(connected && c.controllerName.equals("XBOX 360 For Windows (Controller)"))
      {
        launchButtons.add(new LaunchButton("Launch",new Message("0",ACTIVELAYER,24),x,y,10,85 + 25*index,45,20)); 
        pg.text("Basic Controller Display",60,100 + 25*index++);
        launchButtons.add(new LaunchButton("Launch",new Message("1",ACTIVELAYER,24),x,y,10,85 + 25*index,45,20));        
        pg.text("Body Rig",60,100 + 25*index++);
      }  */    
    }
    
    pg.stroke(TEXTCOLOR);
    pg.line(w/2,50,w/2,h-50);


    //body template!
    int bodyX = 450;
    int bodyY = 75;
    launchButtons.add(new LaunchButton("",new Message("3",ACTIVELAYER,24),x,y,bodyX-50,bodyY-10,100,210));                
    launchButtons.add(new LaunchButton("",new Message("4",ACTIVELAYER,24),x,y,510,65,75,75));                
    launchButtons.add(new LaunchButton("CUSTOM",new Message("5",ACTIVELAYER,24),x,y,510,150,75,30));
    launchButtons.add(new LaunchButton("",new Message("1",ACTIVELAYER,24),x,y,510,190,75,85));
    launchButtons.add(new LaunchButton("XBC Display", new Message("0",ACTIVELAYER,24),x,y,595,65,75,30));
    launchButtons.add(new LaunchButton("LDA Display", new Message("2",ACTIVELAYER,24),x,y,595,105,75,30));
    
    for(int i = 0; i < launchButtons.size(); i++)
    {
      LaunchButton l = (LaunchButton)launchButtons.get(i);
      l.mDraw(pg);
    }
    
    
    //body button
    pg.pushMatrix();
    pg.translate(bodyX,bodyY);
    pg.stroke(BUTTONSTROKE);    
    pg.fill(BUTTONFILL);    
    pg.rect(-15,0,30,30);
    pg.rect(-30,30,60,100);
    pg.rect(-45,30,15,100);
    pg.rect(30,30,15,100);  
    pg.rect(-25,130,25,65);
    pg.rect(0,130,25,65);
    pg.popMatrix();  
  
    //face button
    pg.rect(520,75,15,15);
    pg.rect(550,75,25,25);
    pg.rect(525,120,45,10);
    
    //spine button
    pg.rect(530,195,35,35);
    pg.rect(525,235,45,5);
    pg.rect(535,245,25,5);
    pg.rect(535,255,25,5);
    pg.rect(535,265,25,5);
    
    
    
    pg.endDraw();   
    pg.modified = true;
    
    return DONE;
  }
  
  void mRelease()
  {
    for(int i = 0; i < launchButtons.size(); i++)
    {
      LaunchButton l = (LaunchButton)launchButtons.get(i);
      l.mRelease();
    }    
  }
}


String get3DigitNum(int i)
{
  if(i < 0) return "000";
  if(i > 1000) return "999";
  if(i < 10) return "00" + i;
  if(i > 9 && i < 100) return "0" + i;
  if(i > 99 && i < 1000) return i + "";

  return "ERROR";
}

class Controller extends Layer
{
  String path;  //relative path to the controller
  int fileNum;  
  
  String controllerName = "Invalid Controller Profile";
  
  boolean recording = false;  //are we recording?
  boolean transmit  = false;  //are we transmitting?

  ArrayList data;
  int startTime = 0;
  dataPoint lastPoint;
  
  //where the filename/transmitting text is displayed
  int dataX;  
  int dataY;

  //these two variables are used to transmit over UDP
  //their lengths should be set in the constructor
  int[] ANALOG;
  boolean[] DIGITAL;

  //should be instantiated with whichever device is desired
  ControllDevice device;

  //should be instantiated with proper # of buttons
  ControllButton[] button;

  //should be instantiated with proper # of analog sliders
  ControllSlider[] slider;
  
  //use this to mark if any of the slider values should be inverted (not really needed any more with the customSliders)
  boolean[] invertSlider;

  Controller(String filename, int f)
  {
    //the values here are x,y,w,h, parent layer
    //you should only need to modify x,y,w,h (but not necessary)
    super(width/2-200,height/2-150,400,300,0); 
    
    //the path to our controller profile
    path = System.getProperty("user.dir") + "\\controllers\\" + filename;

    //which fileNum are we on--useful if using more than one input scheme
    fileNum = f;
    
    dataX = w/2;
    dataY = h-50;

    String cName = "";
    int numDigital = 0;
    int numAnalog = 0;
    String[] dNames = new String[0];
    String[] aNames = new String[0];

    String[] preData = loadStrings(path);
    try {
      int index = 0;
      cName = preData[index++];
      controllerName = cName;
      numDigital = Integer.parseInt(preData[index++]);
      numAnalog = Integer.parseInt(preData[index++]);
      dNames = new String[numDigital];
      aNames = new String[numAnalog];
      for(int i = 0; i < numDigital; i++)
      {
        dNames[i] = preData[index++];
      }
      for(int i = 0; i < numAnalog; i++)
      {
        aNames[i] = preData[index++];
      }      
     
      ANALOG = new int[numAnalog];
      DIGITAL = new boolean[numDigital];  

      device = controll.getDevice(cName);

      device.setTolerance(0.05f);
    
      button = new ControllButton[numDigital];
      for(int i =0; i < numDigital; i++)
      {
        button[i] = device.getButton(dNames[i]); 
      }     

      slider = new ControllSlider[numAnalog];
      for(int i = 0; i < numAnalog; i++)
      {
        slider[i] = device.getSlider(aNames[i]);      
      }
    } catch (Exception e) {
      device = null;
      slider = new ControllSlider[0];
      button = new ControllButton[0];
      return;
    }
   
  }

  boolean mDraw()
  {
    if(button.length == 0 && slider.length == 0)
    {
      pg.beginDraw();
      pg.smooth();
      pg.background(BORDERCOLOR);
      pg.fill(FILLCOLOR);
      pg.noStroke();
      pg.rect(3,3,pg.width-6,pg.height-6);
      pg.fill(TEXTCOLOR);
      pg.textFont(font,16);
      pg.textAlign(CENTER);      
      pg.text("Error loading controller.",w/2,h/2-20);
      pg.textFont(font,12);
      pg.text(" Make sure it is plugged in and you loaded the correct controller profile.",w/2,h/2+10);
      //draw the close button
      pg.textFont(font,18);
      pg.stroke(BUTTONSTROKE);
      pg.strokeWeight(1);
      if(mouseX > x+w-30 && mouseX < x+w-5 && mouseY > y+5 && mouseY < y+25)
      {
        if(mousePressed) pg.fill(BUTTONDOWN);
        else pg.fill(BUTTONOVER);
      }else{
        pg.fill(BUTTONFILL);
      }
      pg.rect(w-30,5,25,25);
      pg.fill(TEXTCOLOR);
      pg.text("X",w-22,25); 
   
      pg.endDraw();
      pg.modified = true;
    }else{
      openDraw();
      controlDraw();
      closeDraw();   
    }

    return DONE; 
  }

  //you shouldn't need to modify this
  void openDraw()
  {
    pg.beginDraw();
    pg.smooth();
    pg.background(BORDERCOLOR);
    pg.fill(FILLCOLOR);
    pg.noStroke();
    pg.rect(3,3,pg.width-6,pg.height-6);
    String filename = PROJECTNAME + get3DigitNum(fileNum);
    pg.fill(TEXTCOLOR);
    pg.textFont(font,16);
    pg.textAlign(CENTER);
    pg.text("Current File: " + filename + ".txt",dataX,dataY);
    if(transmit) pg.text("Transmitting...", dataX,dataY+30);
  }

  void controlDraw()
  {
    //put all of your controller-drawing code here (if you have any)
    //be sure to draw everything to the PGraphics variable "pg" 
  }

  //you shouldn't need to modify this
  void closeDraw()
  { 
    //draw the close button
    pg.textFont(font,18);
    pg.stroke(BUTTONSTROKE);
    pg.strokeWeight(1);
    if(mouseX > x+w-30 && mouseX < x+w-5 && mouseY > y+5 && mouseY < y+25)
    {
      if(mousePressed) pg.fill(BUTTONDOWN);
      else pg.fill(BUTTONOVER);
    }else{
      pg.fill(BUTTONFILL);
    }
    pg.rect(w-30,5,25,25);
    pg.fill(TEXTCOLOR);
    pg.text("X",w-22,25);  
      
    
    if(recording){
      pg.fill(255,0,0);
      pg.ellipse(w-30,h-30,30,30);
      dataPoint dp = getDataPoint();
      if(lastPoint.isDifferent(dp))
      {
        lastPoint = dp;     
        data.add(dp); 
      }
      pg.fill(255);
    }

    if(transmit)
    {
      //update every 1/20 of a second ATM...
      if(millis() - LASTUPDATE > 50){
        if(!recording) getDataPoint();
        transmitUDP(DIGITAL, ANALOG);
        LASTUPDATE = millis(); 
      }
    }

    pg.endDraw();
    pg.modified = true;
  }
  
  //assuming your device is a controller, this method will cycle through
  //the proper number of analog/digital inputs (as set by the size of the
  //button and slider arrays) and build a dataPoint
  dataPoint getDataPoint(){
    for(int i = 0; i < ANALOG.length; i++)
    {
      ANALOG[i] = getByteValue(slider[i].getValue()*(invertSlider[i]?-1:1));
      //just in case
      ANALOG[i] = constrain(ANALOG[i],0,255);
    }
    
    int[] dig = new int[DIGITAL.length];
    Arrays.fill(dig,0);
    for(int i = 0; i < 10; i++){
      DIGITAL[i] = button[i].pressed();
      if(DIGITAL[i]) dig[i] = 1;   
    }

    return new dataPoint(0,(int)(millis()-startTime), dig, ANALOG);
  }
  
  void mRelease()
  {
    if(mouseX > x+w-30 && mouseX < x+w-5 && mouseY > y+5 && mouseY < y+25)
    {
      messages.add(new Message(fileNum + "", ACTIVELAYER-1,21));
      DONE = true;      
    }    
  }

  void mKeyUp()
  {
    if(ANALOG.length == 0 || DIGITAL.length == 0)
    {
      popWarn("Must have at least one value to send/save!");
      return;
    }
    
    if(key == ' '){
      recording = !recording;
      if(recording){
        startTime = millis();
        data = new ArrayList();
        lastPoint = getDataPoint();
        data.add(lastPoint);
      }
      else{
        writeState(); 
      }
    }

    if(key == 't'){
      transmit = !transmit;
    }

    if(keyCode == ESC){
      DONE = true;
      //tell the parent layer which fileNum we got up to
      messages.add(new Message(fileNum + "", ACTIVELAYER-1,21));
    }
  }

  void writeState()
  {
    String[] txt = new String[data.size() + 3];
    txt[0] = "1";  //all standard input files have only one controller
    txt[1] = DIGITAL.length + "";
    txt[2] = ANALOG.length + "";
    for(int i = 0; i < data.size(); i++)
    {
      dataPoint dp = (dataPoint)data.get(i);
      txt[i+3] = dp.getString(true);
      println(txt[i+3]);
    }
    //saveStrings...    
    String filename = PROJECTNAME + get3DigitNum(fileNum);
    File out = new File(System.getProperty("user.dir") + "\\data\\" + filename + ".txt");  
    try{
      FileOutputStream outStream = new FileOutputStream(out);
      saveStrings(outStream, txt); 
    }
    catch(FileNotFoundException e){
      popWarn("Save failed!");
    }
    
    //now clear the data from memory and increment our fileNum
    data = new ArrayList();    
    fileNum++;
    
    //it would be good to auto-load these into the fileBin, but (for now) I don't want to have
    //to deal with what happens if you overwrote an older file and that file has instances on 
    //the timeline
  }  



}

/***************
 * gamePadPop does the same thing, but for the Logitech Dual Analog
 ***************/
class gamepadPop extends Controller{

  gamepadPop(int f)
  {
    super("lda.txt",f);
    invertSlider = new boolean[] {false,true,false,true,true};    

  }

  void controlDraw()
  {
    pg.strokeWeight(1);

    //draw the buttons (not strictly neccessary, but I like to give visual feedback)
    color[] buttonColor = {color(0,255,0), color(255,0,0), color(0,0,255), color(255,255,0), color(128), color(128),color(128),color(128),color(128),color(128),color(128),color(128)};
    int[] buttonX = {300,325,275,300,80,300,165,190,80,240,80,300};
    int[] buttonY = {150,125,125,100,60,60,125,125,125,180,20,20};
    
    for(int i = 0; i < button.length; i++){
      pg.noStroke();
      if (button[i].pressed())
      {
        pg.stroke(255,255,255); 
        pg.strokeWeight(2);
      }
      pg.fill(buttonColor[i]);
      pg.ellipse(buttonX[i],buttonY[i],25,25);
    }


    ///draw the D-Pad...just for vanity's sake, I'm not transmitting it
    pg.noStroke();
    pg.fill(128);
    //left
    pg.rect(100,170,20,16);
    //up
    pg.rect(120,145,16,20);
    //right
    pg.rect(136,170,20,16);
    //down
    pg.rect(120,190,16,20);

    //now do the two analog joysticks
    pg.strokeWeight(5);
    pg.strokeCap(ROUND);
    pg.fill(255);
    pg.stroke(255);
    pg.line(80,125,80 + slider[0].getValue()*30, 125 + slider[1].getValue()*30);
    pg.line(240,180,240 + slider[2].getValue()*30, 180 + slider[3].getValue()*30);
  }

}

/***************
 * xbcPop is just a basic visualization for an xbox controller
 ***************/
class xbcPop extends Controller{

  xbcPop(int f)
  {
    super("xbc.txt",f);
    
    invertSlider = new boolean[] {false,true,false,true,true};    

  }

  void controlDraw()
  {
    pg.strokeWeight(1);

    //draw the buttons (not strictly neccessary, but I like to give visual feedback)
    color[] buttonColor = {color(0,255,0), color(255,0,0), color(0,0,255), color(255,255,0), color(128), color(128),color(128),color(128),color(128),color(128)};
    int[] buttonX = {300,325,275,300,80,300,165,190,80,240};
    int[] buttonY = {150,125,125,100,60,60,125,125,125,180};
    
    for(int i = 0; i < button.length; i++){
      pg.noStroke();
      if (button[i].pressed())
      {
        pg.stroke(255,255,255); 
        pg.strokeWeight(2);
      }
      pg.fill(buttonColor[i]);
      pg.ellipse(buttonX[i],buttonY[i],25,25);
    }


    ///draw the D-Pad...just for vanity's sake, I'm not transmitting it
    pg.noStroke();
    pg.fill(128);
    //left
    pg.rect(100,170,20,16);
    //up
    pg.rect(120,145,16,20);
    //right
    pg.rect(136,170,20,16);
    //down
    pg.rect(120,190,16,20);

    //now do the two analog joysticks
    pg.strokeWeight(5);
    pg.strokeCap(ROUND);
    pg.fill(255);
    pg.stroke(255);
    pg.line(80,125,80 + slider[0].getValue()*30, 125 + slider[1].getValue()*30);
    pg.line(240,180,240 + slider[2].getValue()*30, 180 + slider[3].getValue()*30);

    //finally do the triggers
    pg.noStroke();
    pg.fill(128);
    pg.rect(40,20,80,20);
    pg.rect(260,20,80,20);
    float tv = slider[4].getValue();
    pg.fill(255);
    if(tv < 0) pg.rect(260,20,abs(80*tv),20);
    if(tv > 0) pg.rect(40,20,80*tv,20);
  }

}

//assumes a range of -1.0 to 1.0
int getByteValue(float f)
{
  return constrain((int)(127 + 128.0 * f),0,255);
}


//as this controller makes use of the standard xbox controller, it
//makes sense to let the parent class handle all the controller setup
class bodyRig extends xbcPop
{
PGraphics view3D;
spineRig body;
boolean[] buttonDown;

bodyRig(int f)
{
  super(f);
  
  //was there an error?
  if(button.length == 0 && slider.length == 0) return;
  
  ANALOG = new int[15];
  DIGITAL = new boolean[] {false};
  
  view3D = createGraphics(300,300,P3D);
  
  body = new spineRig(new int[] {20,20,20,20,20});
  
  ControllButton[] oldButton = button;
  button = new ControllButton[5];  //i only want 5 of them
  for(int i = 0; i < 5; i++){button[i] = oldButton[i];}

  buttonDown = new boolean[5];
  Arrays.fill(buttonDown,false);
}

void controlDraw()
{
  
  for(int i = 0; i < button.length; i++)
  {
    if(buttonDown[i] && !button[i].pressed()) body.setSpineLock(i + 1);
    buttonDown[i] = button[i].pressed();
  }
      
  //body.setSpinePermissions(new boolean[] {XBoxButtons[0].pressed(),XBoxButtons[1].pressed(),XBoxButtons[2].pressed(),XBoxButtons[3].pressed()});
  body.setSpineRot(slider[2].getValue(), -slider[1].getValue(), slider[0].getValue());
  body.adjustMult(slider[4].getValue() / 10);
  
  view3D.beginDraw();
  view3D.background(0,0,0,0);
  body.mDraw(view3D);
  view3D.endDraw();
  view3D.modified = true;
  
  for(int i = 0; i < view3D.pixels.length; i ++){
      color c = view3D.pixels[i];
      if(c != color(0,0,0,0)) view3D.pixels[i] = color(red(c), green(c), blue(c));
  }
  
  pg.image(view3D,w/2-150,10);
}

  dataPoint getDataPoint()
  {
    int[] dig = new int[] {0};
    String s = "Object Level: ";
    for(int i = 0; i < body.spine.length; i++)
    {
      ANALOG[i*3] = angleToByte(body.spine[i].x);
      ANALOG[i*3 + 1] = angleToByte(body.spine[i].y);
      ANALOG[i*3 + 2] = angleToByte(body.spine[i].z);
      s += ANALOG[i*3] + " " + ANALOG[i*3 + 1] + " " + ANALOG[i*3 + 2] + " ";
    }

    return new dataPoint(0,(int)(millis()-startTime), dig, ANALOG);
  }
}

//it would eventually be nice to make a 4-bone spine rig and a 2-bone spine rig, but for now
//just the 4-bone will be plenty (plus the neck!)
class spineRig
{
  int lockedUp = 0;
  Vector curState = new Vector(0,0,0); //holds our analog values
  Vector[] spine;  //using x/y/z as H/P/B  (rotY,rotX,rotZ)
  int[] boneLength;
  boolean[] locked;
  float mult = PI/2.0;
  int x = 150;
  int y = 175;
  
  spineRig(int[] ln)
  {
    spine = new Vector[5];
    boneLength = new int[5];
    locked = new boolean[5];
    for(int i = 0; i < 5; i++)
    {
      spine[i] = new Vector(0,0,0);
      boneLength[i] = ln[i];
      locked[i] = false;
    } 
  }
  
  void mDraw(PGraphics pg)
  {
    pg.pushMatrix();  //offset
    pg.translate(x,y);
      
      for(int i = 0; i < spine.length; i++)
      {
        //println(spine[i].x + " " + spine[i].y + " " + spine[i].z);
        pg.rotateY(spine[i].x);
        pg.rotateX(spine[i].y);
        pg.rotateZ(spine[i].z);
        pg.noStroke();
        if(locked[i]) pg.fill(255,0,0,255);
        else pg.fill(255,255,255,255);
        pg.sphere(8);
        pg.stroke(255,255,255,255);
        pg.line(0,0,0,0,-boneLength[i],0);
        pg.translate(0,-boneLength[i],0);  //minus fix for y-pos
        
        if(i == spine.length-2)
        {
          //"shoulders"
          pg.pushMatrix();
          pg.translate(0,boneLength[i],0);
          pg.scale(8,1,1);
          pg.box(10);
          pg.popMatrix();
        }
      }
      
    //draw a "head"
    pg.noStroke();
    pg.sphere(25);
    pg.popMatrix();
    pg.fill(0,0,0,255);
    pg.stroke(255,255,255,255);
    pg.rect(0,0,299,30);
    pg.fill(255,255,255,255);
    pg.rect(0,0,degrees(mult)/180 * 300,30);
  }
  
  void setSpineLock(int l)
  {
    Arrays.fill(locked,false);
    if(lockedUp == l)
    {
      lockedUp = 0;
      return;  //deactivate
    }
    lockedUp = l;
    //otherwise set to the new lock
    int lock = constrain(l,0,spine.length);
    for(int i = 0; i < lock; i++)
    {
      locked[i] = true;
    }
  }
  
  void setSpineRot(float h, float p, float b)
  {
    curState.x = h;
    curState.y = p;
    curState.z = b;
    
    for(int i = 0; i < spine.length; i++)
    {
      if(!locked[i])
      {
        spine[i].x = curState.x * mult;
        spine[i].y = curState.y * mult;
        spine[i].z = curState.z * mult;
      }
    }
  }
  
  void adjustMult(float v)
  {
    mult -= v; 
    mult = constrain(mult,0,2*PI);
  } 
}

class Vector
{
 float x,y,z;
 
 Vector(float _x, float _y, float _z) 
 {
  x = _x;
  y = _y;
  z = _z; 
 }

}

//In-engine blends run from -180 to 180...
int angleToByte(float f)
{
  //first convert it to an analog value (-1 to 1)
  float ang = f;
  while(ang < -PI) {ang += 2*PI;}
  while(ang > PI) {ang -= 2*PI;}
  
  return getByteValue(ang/PI);
}


float getAngle(float x1,float y1,float x2,float y2)  // x1/y1 should be the center point...  x2/y2 the rotational point
{
float x = x2 - x1;
float y = y2 - y1;

int quadrant = 0;
if (x > 0 && y > 0) quadrant = 1;
if (x > 0 && y < 0) quadrant = 2;
if (x < 0 && y < 0) quadrant = 3;
if (x < 0 && y > 0) quadrant = 4;

float r = dist(x,y,x2,y2);
float theta = 0;

theta = aTan(x,y);
//println(degrees(theta));
return theta;
}

float aTan(float tx, float ty) { 
  float theta = abs(atan( ty / tx )); 
   
  if(tx < 0 && ty > 0) // Quadrant II 
    theta = PI - theta; 
  else if(tx < 0 && ty < 0) // Quadrant III 
    theta = PI + theta; 
  else if(tx > 0 && ty < 0) // Quadrant IV 
    theta = TWO_PI - theta; 
     
  if(ty == 0 && tx > 0) 
    theta = 0; 
  else if(tx == 0 && ty > 0) 
    theta = PI / 2; 
  else if(tx == 0 && ty < 0) 
    theta = (3.0 / 2.0) * PI; 
  else if(ty == 0 && tx < 0) 
    theta = PI; 
     
  return theta; 
}
